/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file    cordic.c
  * @brief   This file provides code for the configuration
  *          of the CORDIC instances.
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "cordic.h"

/* USER CODE BEGIN 0 */
#define PI         3.14159265359f
#define Q31_SCALE (1U << 31)
/* USER CODE END 0 */

CORDIC_HandleTypeDef hcordic;

/* CORDIC init function */
void MX_CORDIC_Init(void)
{

  /* USER CODE BEGIN CORDIC_Init 0 */

  /* USER CODE END CORDIC_Init 0 */

  /* USER CODE BEGIN CORDIC_Init 1 */

  /* USER CODE END CORDIC_Init 1 */
  hcordic.Instance = CORDIC;
  if (HAL_CORDIC_Init(&hcordic) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN CORDIC_Init 2 */

  /* USER CODE END CORDIC_Init 2 */

}

void HAL_CORDIC_MspInit(CORDIC_HandleTypeDef* cordicHandle)
{

  if(cordicHandle->Instance==CORDIC)
  {
  /* USER CODE BEGIN CORDIC_MspInit 0 */

  /* USER CODE END CORDIC_MspInit 0 */
    /* CORDIC clock enable */
    __HAL_RCC_CORDIC_CLK_ENABLE();

    /* CORDIC interrupt Init */
    HAL_NVIC_SetPriority(CORDIC_IRQn, 5, 0);
    HAL_NVIC_EnableIRQ(CORDIC_IRQn);
  /* USER CODE BEGIN CORDIC_MspInit 1 */

  /* USER CODE END CORDIC_MspInit 1 */
  }
}

void HAL_CORDIC_MspDeInit(CORDIC_HandleTypeDef* cordicHandle)
{

  if(cordicHandle->Instance==CORDIC)
  {
  /* USER CODE BEGIN CORDIC_MspDeInit 0 */

  /* USER CODE END CORDIC_MspDeInit 0 */
    /* Peripheral clock disable */
    __HAL_RCC_CORDIC_CLK_DISABLE();

    /* CORDIC interrupt Deinit */
    HAL_NVIC_DisableIRQ(CORDIC_IRQn);
  /* USER CODE BEGIN CORDIC_MspDeInit 1 */

  /* USER CODE END CORDIC_MspDeInit 1 */
  }
}

/* USER CODE BEGIN 1 */

float Calculate_Sine(float angle)
{
  // printf("Input angle = %f\n", angle);
    CORDIC_ConfigTypeDef sConfig;
    int32_t input;
    int32_t output;

    // 将角度转换为-π到π范围内
    while (angle > PI)
    {
        angle -= 2 * PI;
    }
    while (angle < -PI)
    {
        angle += 2 * PI;
    }

    // 将角度转换为CORDIC输入格式（Q1.31）
    input = (int32_t)(angle * Q31_SCALE / PI);

    // 配置CORDIC参数
    sConfig.Function = CORDIC_FUNCTION_SINE; // 设置为正弦计算
    sConfig.Precision = CORDIC_PRECISION_6CYCLES; // 设置计算精度
    sConfig.Scale = CORDIC_SCALE_0; // 设置缩放因子
    sConfig.NbWrite = CORDIC_NBWRITE_1; // 设置写入次数
    sConfig.NbRead = CORDIC_NBREAD_1; // 设置读取次数
    sConfig.InSize = CORDIC_INSIZE_32BITS; // 设置输入数据大小
    sConfig.OutSize = CORDIC_OUTSIZE_32BITS; // 设置输出数据大小

    // 初始化CORDIC配置
    if (HAL_CORDIC_Configure(&hcordic, &sConfig) != HAL_OK)
    {
        Error_Handler();
    }

    // 启动CORDIC计算
    if (HAL_CORDIC_Calculate(&hcordic, &input, &output, 1, 10) != HAL_OK)
    {
        Error_Handler();
    }

    // 将输出值转换为浮点数
    float sine_value = (float)output / (1 << 31);
    // 处理计算结果
    return -sine_value;
}

float Calculate_Cosine(float angle)
{
    // printf("Input angle = %f\n", angle);
    CORDIC_ConfigTypeDef sConfig;
    int32_t input;
    int32_t output;

    // 将角度转换为-π到π范围内
    while (angle > PI)
    {
        angle -= 2 * PI;
    }
    while (angle < -PI)
    {
        angle += 2 * PI;
    }

    // 将角度转换为CORDIC输入格式（Q1.31）
    input = (int32_t)(angle * Q31_SCALE / PI);

    // 配置CORDIC参数
    sConfig.Function = CORDIC_FUNCTION_COSINE; // 设置为余弦计算
    sConfig.Precision = CORDIC_PRECISION_6CYCLES; // 设置计算精度
    sConfig.Scale = CORDIC_SCALE_0; // 设置缩放因子
    sConfig.NbWrite = CORDIC_NBWRITE_1; // 设置写入次数
    sConfig.NbRead = CORDIC_NBREAD_1; // 设置读取次数
    sConfig.InSize = CORDIC_INSIZE_32BITS; // 设置输入数据大小
    sConfig.OutSize = CORDIC_OUTSIZE_32BITS; // 设置输出数据大小

    // 初始化CORDIC配置
    if (HAL_CORDIC_Configure(&hcordic, &sConfig) != HAL_OK)
    {
        Error_Handler();
    }

    // 启动CORDIC计算
    if (HAL_CORDIC_Calculate(&hcordic, &input, &output, 1, 10) != HAL_OK)
    {
        Error_Handler();
    }

    // 将输出值转换为浮点数
    float cosine_value = (float)output / (1 << 31);
    // 处理计算结果
    return -cosine_value;
}

/* USER CODE END 1 */
